import numpy as np
from  draw_map import visualize_line, visualize_line1


class PointsAttribue:
    def __init__(self, fuse_xyz, fuse_rpy):
        self.x, self.y, self.z = fuse_xyz
        self.r, self.p, self.yaw = fuse_rpy
        self.size = 0.05
        self.global_gird_x = int(self.x / 0.05)
        self.global_gird_y = int(self.y / 0.05)

        # 用来存储障碍物坐标的全局xy位置和相对零点的栅格位置：
        self.OBTS_WORLDS_GLOBAL = {}

    def get_global_xyz(self):
        return(self.x, self.y, self.z)
    
    def get_global_grid_xy(self):
        return (self.global_gird_x, self.global_gird_y, 0)
    

    def get_tansform_points(self, points):
        for point in points:
            px,py,pz = point
            if not (-self.SIZE <= px <= self.SIZE and -self.SIZE <= py <= self.SIZE):
                continue
            if pz < -0.05:
                continue
            obts_world, obts_global_grid = self.transform_point_matrix(point)
            self.OBTS_WORLDS_GLOBAL[obts_global_grid] = obts_world



    
    #相对坐标转 世界坐标 平移量 和栅格数目
    def transform_point_matrix(self, local_points):
        yaw = self.yaw
        x_local, y_local = local_points
        # 创建旋转矩阵
        rotation_matrix = np.array([
            [np.cos(yaw), -np.sin(yaw)],
            [np.sin(yaw), np.cos(yaw)]
        ])

        # 创建局部坐标的列向量
        local_point = np.array([x_local, y_local])

        # 应用旋转矩阵
        rotated_point = np.dot(rotation_matrix, local_point)

        grid_size = self.size
        # 在获得 grid_x, grid_y

        # 障碍物点平移到世界坐标系
        x_world = self.x + rotated_point[0]
        y_world = self.y + rotated_point[1]

        # grid_x = self.CENTER[0] + int(rotated_point[0] / grid_size)
        # grid_y = self.CENTER[1] + int(rotated_point[1] / grid_size)
        global_obts_grid_x = int(x_world / self.size)
        global_obts_grid_y = int(y_world / self.size)
        

        # return x_world, y_world, rotated_point[0], rotated_point[1],  grid_x, grid_y
        return (x_world, y_world), (global_obts_grid_x, global_obts_grid_y)
    


    
    def get_local_relative_xy(self, cur_postion_grid_xyz):
        x, y, z, = cur_postion_grid_xyz

        delta_x = self.global_gird_x - x
        delta_y = self.global_gird_y - y

        return (delta_x, delta_y, 0)


  
class BuildGridMap:
    def __init__(self, lidar_data):
        self.lidar_data = lidar_data
        self.global_size = (0, 0)  # 初始化全局尺寸
        self.x_min, self.y_min, self.x_max, self.y_max = 0,0,0,0
        self.grid_size = 0.05
        self.calculate_map_grid_size()
        self.GLOBAL_MAP = np.full(self.global_size, 0.5)  # 初始占用概率为 0.5
        print(f'Global size {self.global_size}')

    def calculate_map_grid_size(self):
        # 提取所有 fusepose 的 x, y
        fusepose_xy = np.array([frame['fusepose'][:2] for frame in self.lidar_data if 'fusepose' in frame])
        
        # 计算 x 和 y 的最小值和最大值
        x_min, y_min = fusepose_xy[:, 0].min(), fusepose_xy[:, 1].min()
        x_max, y_max = fusepose_xy[:, 0].max(), fusepose_xy[:, 1].max()
        self.x_min, self.y_min, self.x_max, self.y_max = x_min, y_min, x_max, y_max
        # 打印结果
        print(f"Fusepose x 范围: 最小值 = {x_min}, 最大值 = {x_max}")
        print(f"Fusepose y 范围: 最小值 = {y_min}, 最大值 = {y_max}")

        # 计算全局尺寸
        global_x = int((x_max - x_min) / self.grid_size  )
        global_y = int((y_max - y_min) / self.grid_size  )
        self.global_size = (global_x, global_y)
        self.global_size = (round(self.global_size[0], -1), round(self.global_size[1], -1))

    def get_global_map(self):
        return self.GLOBAL_MAP
    
    def get_global_size(self):
        return self.global_size
    
    def xyz2xy(self, x,y,z):
        x1 = (x - self.x_min) / self.global_size
        y1 = (y - self.y_min) / self.global_size
        x1 = int(x1)
        y1 = int(y1)
    
        return x1,y1


class updateMap():
    def __init__(self, LOCAL_MAP, GLOBAL_MAP, local_map_size = (100,100)):
        self.EMPTY_LOCAL_MAP = LOCAL_MAP
        self.BEFORE_LOCAL_MAP = LOCAL_MAP
        self.UPDATE_LOCAL_MAP = LOCAL_MAP
        self.global_map = GLOBAL_MAP
        self.fusepose_xyz_sets = []
        self.fusepose_rpy_sets = []
        self.local_map_size = local_map_size
        self.LOCAL_MAP_SETS = []
        self.xyz_sets = []
        self.rpy_sets = []
        self.points_sets = []
        self.world_points_sets = []
        self.relateive_points_sets = []
        self.grid_sets = []
        self.SIZE = 2
        self.CENTER = 50, 50
        self.MINI_GRID = 0.05

    def jude_obts_in(self, obts_postion, fuse_postiion):
        x0,y0,z0 = fuse_postiion
        x1,y1 = x0 - self.SIZE, y0 - self.SIZE
        x2,y2 = x0 + self.SIZE, y0 + self.SIZE
        x,y,z = obts_postion
        if min(x1, x2) <= x <= max(x1, x2) and min(y1, y2) <= y <= max(y1, y2):
            return True
    
        return False
        # pass

    def obts_grid_xy_2_new_xy(self, points, fusepsoe_xyz, fusepose_rpy, i):
        pass





    def update_local_map(self, points, fusepsoe_xyz, fusepose_rpy, i):

        before_map = self.BEFORE_LOCAL_MAP  # 初始占用概率为 0.5
        update_map = self.EMPTY_LOCAL_MAP

        before_grid_xy_sets = []
        if i == 0:
            pass
        else:
            befor_world_points = self.world_points_sets[i-1]
            before_grid_points = self.grid_sets[i-1]
            keeping_points = []
            # for points in befor_world_points:
            for world_points, relative_points in zip(befor_world_points, before_grid_points):
                old_grid_x, old_grid_y = relative_points
                if old_grid_x >=100 or old_grid_y >=100:
                    continue
                # x_world, y_world, ppx, ppy, grid_x, grid_y = self.transform_point_matrix(world_points, fusepsoe_xyz, fusepose_rpy)
                if self.jude_obts_in(world_points, fusepsoe_xyz) == False:
                    continue

                else:
                    grid_x, grid_y = self.world2gird(world_points, fusepsoe_xyz)

                # x = int(map_x // 2 - ppx * 20)
                # y = int(map_y // 2 - ppy * 20)
                # if grid_x >=80 or grid_y >= 80:
                #     continue
                # before_grid_points
                obts_center = (grid_x,grid_y)
                before_grid_xy_sets.append(obts_center)
                # update using probality
                update_map[obts_center] = before_map[(old_grid_x, old_grid_y)]
                # update_map[obts_center] = before_map[(old_grid_x, old_grid_y)]


        map_x, map_y = self.local_map_size
        points_tmp = []
        points_world = []
        points_relateive = []
        grid_postion = []



        for point in points:
            px, py, pz = point
            if not (-self.SIZE <= px <= self.SIZE and -self.SIZE <= py <= self.SIZE):
                continue
            if pz < -0.05:
                continue
            
            points_tmp.append((px,py,pz))
            # 获得全局坐标和相对坐标
            x_world, y_world, ppx, ppy, grid_x, grid_y = self.transform_point_matrix(point, fusepsoe_xyz, fusepose_rpy)

            points_relateive.append((ppx,ppy))
            points_world.append((x_world, y_world, 0))
            grid_postion.append((grid_x, grid_y))
            if (grid_x, grid_y)  in before_grid_xy_sets:
                # x = int(map_x // 2 - ppx * 20)
                # y = int(map_y // 2 - ppy * 20)
                if grid_x >=100 or grid_y >= 100:
                    continue
                obts_center = (grid_x,grid_y)
                # update using probality
                update_map[obts_center] = self.update_probability(update_map[obts_center], 0.8, 0.2)
            
            else:
                if grid_x >=100 or grid_y >= 100:
                        continue
                obts_center = (grid_x,grid_y)
                update_map[obts_center] = self.update_probability(update_map[obts_center], 0.8, 0.2)
                    # update_map[obts_center] = self.update_probability(update_map[obts_center], 0.4, 0.6)

            # update_map[obts_center] = 0.9
        
        # 局部图中的障碍物栅格坐标
        self.grid_sets.append(grid_postion)
        self.relateive_points_sets.append(points_relateive)
        # 障碍物的世界坐标系的点
        self.world_points_sets.append(points_world)
        self.xyz_sets.append(fusepsoe_xyz)
        self.rpy_sets.append(fusepose_rpy)
        self.points_sets.append(points_tmp)


        self.UPDATE_LOCAL_MAP = update_map
        self.BEFORE_LOCAL_MAP = update_map
        self.LOCAL_MAP_SETS.append(update_map)
        self.fusepose_rpy_sets.append(fusepose_rpy)

    def update_probability(self, P_O, P_Z_given_O, P_Z_given_not_O):
        P_Z = P_Z_given_O * P_O + P_Z_given_not_O * (1 - P_O)
        P_O_given_Z = (P_Z_given_O * P_O) / P_Z
        return P_O_given_Z
        # grid[center] = update_probability(grid[center], P_Z_given_O, P_Z_given_not_O)
    

    def world2gird(self, world_point, fusepose_xyz):
        x,y,z = world_point
        x1,y1,z1 = fusepose_xyz

        grid_x = self.CENTER[0] + (x - x1) / self.MINI_GRID
        grid_y = self.CENTER[0] + (y - y1) / self.MINI_GRID

        return int(grid_x), int(grid_y)


    def update_global_map(self):
        pass


    def visual_index_local_map(self, i):
            grid_map = self.LOCAL_MAP_SETS[i]
            fusepose_rpy = self.fusepose_rpy_sets[i]
            # visualize_line(grid_map ,i)
            visualize_line1(grid_map , fusepose_rpy, i)

    def get_map_xyz_rpy(self):
        return self.LOCAL_MAP_SETS, self.xyz_sets, self.rpy_sets, self.points_sets, self.relateive_points_sets, self.world_points_sets